// --------------------------------------------------------------------------------------------------------------------
// <copyright file="VelocityControlledJointPositionController.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Manipulation.JointController
{
    using System;
    using System.Collections.Generic;
    using System.Linq;
    using System.Text;
    using System.Threading.Tasks;

    using Microsoft.Robotics.Core.Algorithms;
    using Microsoft.Robotics.Manipulation;

    /// <summary>
    /// Control joint positions via velocities
    /// </summary>
    public class VelocityControlledJointPositionController : JointController
    {
        /// <summary>
        /// Pid controller to achieve target position given 
        /// </summary>
        private IGenericPidController[] pidControllers;

        /// <summary>
        /// Optional deadband
        /// </summary>
        private double[] deadband;

        /// <summary>
        /// Optional maximum delta between successive outputs
        /// </summary>
        private double[] maxDelta;

        /// <summary>
        /// Previous output 
        /// </summary>
        private double[] previousOutput;

        /// <summary>
        /// Maximum absolute value of output
        /// </summary>
        private double[] maxAbsoluteOutput;

        /// <summary>
        /// Initializes a new instance of the <see cref="VelocityControlledJointPositionController"/> class.
        /// </summary>
        /// <param name="numJoints">Number of joints.</param>
        /// <param name="pidController">PID controller for each joint.</param>
        /// <param name="deadband"> Controller deadbands.  </param>
        /// <param name="maxDelta"> Controller output max delta (per cycle). </param>
        /// <param name="maxAbsoluteOutput"> Maximum absolute controller output. </param>
        public VelocityControlledJointPositionController(
            int numJoints, 
            IGenericPidController[] pidController,
            double[] deadband,
            double[] maxDelta,
            double[] maxAbsoluteOutput)
            : base(numJoints)
        {
            if (pidController.Count() != numJoints)
            {
                throw new ArgumentException("pidControllers must have a number of elements equal to numJoints");
            }

            this.pidControllers = new IGenericPidController[this.NumJoints];
            this.deadband = deadband;
            this.maxDelta = maxDelta;
            this.maxAbsoluteOutput = maxAbsoluteOutput;
            this.previousOutput = new double[this.NumJoints];

            Array.Copy(pidController, this.pidControllers, this.NumJoints);

            this.ReferenceSignalType = JointSignalType.Position;
            this.ControlOutputSignalType = JointSignalType.Velocity;
        }

        /// <summary>
        /// Execute one cycle of the controller.
        /// </summary>
        /// <param name="status">Current manipulator status.</param>
        /// <returns>Velocity references.</returns>
        public override double[] Cycle(ManipulatorStatus status)
        {
            double[] output = new double[this.NumJoints];

            if (status.JointPositions.Count() != this.NumJoints)
            {
                throw new ArgumentException("Manipulator status does not contain the expected number of joints");
            }

            lock (this.ReferenceLockObject)
            {
                for (int i = 0; i < this.NumJoints; ++i)
                {
                    double error = status.JointPositions[i] - this.ReferenceNoCopy[i];

                    // Always run, even if within deadband so damper computations will be correct
                    output[i] = this.pidControllers[i].Evaluate(status.JointPositions[i] - this.ReferenceNoCopy[i], (long)(status.TimeStampinMS * 1000));

                    if (this.deadband != null && Math.Abs(error) < this.deadband[i])
                    {
                        output[i] = 0;
                    }
                    else if (this.maxDelta != null && Math.Abs(output[i] - this.previousOutput[i]) > this.maxDelta[i])
                    {
                        output[i] = this.previousOutput[i] - Math.Sign(error) * this.maxDelta[i];
                    }

                    this.previousOutput[i] = output[i];

                    if (Math.Abs(output[i]) > this.maxAbsoluteOutput[i])
                    {
                        output[i] = Math.Sign(output[i]) * this.maxAbsoluteOutput[i];
                    }
                }
            }

            return output;
        }
    }
}
